#include <dirent.h>

#include <algorithm>
#include <fstream>
#include <iostream>

#include "cvisible_corrector.hpp"
using namespace cv;
using namespace std;
const char* kDataPath = "../debug_data_visible";
std::vector<std::string> GetFileList(void);
void ProcessFrames(std::vector<std::string>& file_list, int num_frames = 50);
CVisibleCorrector Corrector;
int main(void) {
  std::vector<std::string> file_list = GetFileList();
  ProcessFrames(file_list);
  return 0;
}
std::vector<std::string> GetFileList(void) {
  std::vector<std::string> file_list;
  // 打开目录
  DIR* dir = opendir(kDataPath);
  if (!dir) {
    std::cerr << "Failed to open directory: " << kDataPath << std::endl;
    return file_list;
  }

  // 读取目录项
  struct dirent* entry;
  while ((entry = readdir(dir)) != nullptr) {
    // 检查是否为文件
    if (entry->d_type == DT_REG) {
      file_list.push_back(std::string(entry->d_name));
    }
    // 如果需要递归遍历子目录，可以在这里添加代码
  }
  // 关闭目录
  closedir(dir);

  // 升序排序
  std::sort(file_list.begin(), file_list.end(),
            [](std::string a, std::string b) {
              return std::stoi(a) < std::stoi(b);  // 升序排序
            });
  for (auto& file_name : file_list) {
    // 构建文件的完整路径
    file_name = std::string(kDataPath) + "/" + file_name;
  }
  return file_list;
}

void ProcessFrames(std::vector<std::string>& file_list, int num_frames) {
  int frame_count = 0;

  for (const auto& file_path : file_list) {
    frame_count++;
    if (frame_count > num_frames) {
      break;
    }
    try {
      Mat img = cv::imread(file_path);
      cout << "Start: " << file_path << endl;
      // 测试角点检测全流程
      static int num_img = 0;
      num_img++;
      bool DEBUG = true;
      vector<Point2f> points;
      CTimer corner_detection_time("Corner Detection");
      CTimer template_matching_time("Template Matching");
      Mat template_img = imread("standard.jpg");
      Mat gray_template;
      cvtColor(template_img, gray_template, COLOR_BGR2GRAY);
      FileStorage fs_read, fs_write;
      vector<Point2f> standard_points, initial_infrared_points, initial_visible_points;
      Mat H2_inv;
      string gCalibrationParamsFile = "calibration_params.xml";

      corner_detection_time.start();
      cout << "Calibration Start" << endl;
      points = Corrector.DetectCornerPoints(img,true);
      cout << "Calibration End" << endl;
      corner_detection_time.end();
      // 尝试使用模板匹配进行目标检测
      if(points.size() == 0){
        template_matching_time.start();
        points = Corrector.DetectTargetAndCorners(img, gray_template, true);
        template_matching_time.end();
      }
      if(points.size() >= 8){
        // 读取出厂标定信息
        fs_read.open(gCalibrationParamsFile, FileStorage::READ);
        if (!fs_read.isOpened()) std::cerr << "Failed to open read:" << gCalibrationParamsFile << std::endl;
        fs_read["StandardPoints"] >> standard_points;
        fs_read["InfraredPoints"] >> initial_infrared_points;
        fs_read["VisiblePoints"] >> initial_visible_points;
        fs_read.release();
        // 使用原来的方式计算 H2_inv
        if (points.size() >= 4 && standard_points.size() >= 4) {
          cout << "points.size(): " << points.size() << ", standard_points.size(): " << standard_points.size() << endl;
          // 3.0是RANSAC算法中的重投影阈值(reprojection threshold)
          // 该阈值用于判断一个点是否为内点，如果重投影误差大于该阈值则认为是外点
          // 阈值越小筛选越严格，这里选择3.0像素作为合理值
          H2_inv = findHomography(points, standard_points, RANSAC, 3.0);
          if(DEBUG){
            // 将当前检测到的点投影到标准图上
            std::vector<cv::Point2f> projected_points;
            cv::perspectiveTransform(points, projected_points, H2_inv);

            // 在template_img上绘制投影后的点
            Mat display_img = template_img.clone();
            for(const auto& pt : projected_points) {
              circle(display_img, pt, 3, Scalar(0,0,255), -1);
            }
            
            // 保存结果图像
            std::string img_Path = "reprojection_result" + std::to_string(num_img) + ".jpg";
            imwrite(img_Path, display_img);
            // 在原图上绘制检测到的角点
            Mat corner_img = img.clone();
            for(const auto& pt : points) {
              circle(corner_img, pt, 5, Scalar(0,0,255), -1);
            }
            
            // 保存结果图像
            std::string corner_img_path = "corner_detection_result" + std::to_string(num_img) + ".jpg";
            imwrite(corner_img_path, corner_img);
          }
        } else {
          cout << "Error: Not enough points for homography calculation. Need at least 4 pairs." << endl;
          cout << "Current points: " << points.size() << ", standard points: " << standard_points.size() << endl;
          break;
        }
      }
      // 仅测试模板匹配
      /*
      Mat template_img = imread("standard.jpg");
      Mat gray_template;
      cvtColor(template_img, gray_template, COLOR_BGR2GRAY);
      vector<Point2f> target_and_corners = Corrector.DetectTargetAndCorners(img, gray_template, true);
      */
      cout << "End" << endl;
    }

    catch (const std::exception& e) {
      std::cerr << "Exception occurred while opening file: " << file_path
                << " - " << e.what() << std::endl;
    }
  }
}